#include "securec.h"
#include <DataBase/DataBase.h>
#include <cmsis_os2.h>
#include <iot_uart.h>
#include <iot_gpio.h>
#include <hi_io.h>
#include <lwip/sockets.h>
#include <stdio.h>

#define TELNET_PORT 23

#define IAC 255 /* FF interpret as command: */

#define DONT 254 /* FE you are not to use option */
#define DO 253   /* FD please, you use option */
#define WONT 252 /* FC I won't use option */
#define WILL 251 /* FB I will use option */

#define SB 250  /* FA interpret as subnegotiation */
#define SE 240  /* F0 end sub negotiation */
#define NOP 241 /* F1 No Operation */

#define TELOPT_ECHO 1   /* 01 echo */
#define TELOPT_SGA 3    /* 03 suppress go ahead */
#define TELOPT_TTYPE 24 /* 18 terminal type */
#define TELOPT_NAWS 31  /* 1F window size */

static void telnet_socket_task(void *arg);
static void telnet_uart_task(void *arg);
static uint8_t proc_IAC(uint8_t data);


int Telnet_init(void)
{
    osThreadAttr_t socket_thread;
    osThreadAttr_t uart_thread;
    
    socket_thread.name = "tn_socket";
    socket_thread.attr_bits = 0U;
    socket_thread.cb_mem = NULL;
    socket_thread.cb_size = 0U;
    socket_thread.stack_mem = NULL;
    socket_thread.stack_size = 2048;
    socket_thread.priority = osPriorityBelowNormal;
    if (osThreadNew((osThreadFunc_t)telnet_socket_task, NULL, &socket_thread) == NULL)
    {
        printf("[Telnet_init] Falied to create tn_socket!\n");
    }

    uart_thread.name = "tn_uart";
    uart_thread.attr_bits = 0U;
    uart_thread.cb_mem = NULL;
    uart_thread.cb_size = 0U;
    uart_thread.stack_mem = NULL;
    uart_thread.stack_size = 2048;
    uart_thread.priority = osPriorityBelowNormal;
    if (osThreadNew((osThreadFunc_t)telnet_uart_task, NULL, &uart_thread) == NULL)
    {
        printf("[Telnet_init] Falied to create tn_uart!\n");
    }

    return 0;
}

static const uint8_t telnet_ctrl[] = {
    IAC, DO,   TELOPT_ECHO, //
    IAC, DO,   TELOPT_NAWS, //
    IAC, WILL, TELOPT_ECHO, //
    IAC, WILL, TELOPT_SGA,  //
};

static uint8_t tcp_recv_buf[128];
static int client_fd = -1;

static void telnet_socket_task(void *arg)
{
    (void) arg;
    int socket_fd = 0;
    int recv_len = 0;
    struct sockaddr_in client_addr = {0};
    struct sockaddr_in local_addr = {0};
    int init = 0;

    socket_fd = lwip_socket(AF_INET, SOCK_STREAM, 0);
    if (socket_fd < 0)
    {
        printf("socket creat failed\r\n");
        return;
    }

    printf("socket creat success\r\n");

    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(TELNET_PORT);
    local_addr.sin_addr.s_addr = INADDR_ANY;

    if (lwip_bind(socket_fd, (struct sockaddr *)&local_addr, sizeof(local_addr)) == -1)
    {
        printf("socket bind failed %d\r\n", errno);
        lwip_close(socket_fd);
        return;
    }
    printf("socket bind success\r\n");

    if (lwip_listen(socket_fd, 1) != 0)
    {
        printf("socket listen failed\r\n");
        lwip_close(socket_fd);
        return;
    }

    printf("socket listen success\r\n");

    while (1)
    {
        socklen_t addr_len = sizeof(client_addr);
        client_fd = accept(socket_fd, (struct sockaddr *)&client_addr, &addr_len);
        if (client_fd < 0)
        {
            continue;
        }
        printf("socket accept %d\r\n", client_fd);
        while ((recv_len = lwip_recv(client_fd, tcp_recv_buf, sizeof(tcp_recv_buf), 0)) > 0)
        {
            if (init == 0)
            {
                lwip_send(client_fd, telnet_ctrl, sizeof(telnet_ctrl), 0);
                init = 1;
                continue;
            }
            uint8_t *wp = tcp_recv_buf;
            int send_len = 0;
            for (int i = 0; i < recv_len; i++)
            {
                uint8_t tmp = proc_IAC(tcp_recv_buf[i]);
                if (tmp != 0xff)
                {
                    *wp++ = tmp;
                    send_len++;
                }
            }
            IoTUartWrite(2, tcp_recv_buf, send_len);
        }
        init = 0;
        lwip_close(client_fd);
        client_fd = -1;
    }
}

static uint8_t uart_read_buf[128];
static uint8_t tcp_send_buf[128];

static void telnet_uart_task(void *arg)
{
    (void) arg;
    int read_num = 0;
    while (1)
    {
        while (client_fd > 0)
        {
            read_num = IoTUartRead(2, uart_read_buf, sizeof(uart_read_buf));
            if (read_num > 0)
            {
                memcpy_s(tcp_send_buf, sizeof(tcp_send_buf), uart_read_buf, read_num);
                lwip_send(client_fd, tcp_send_buf, read_num, MSG_DONTWAIT);
            }
        }
        osDelay(5);
    }
}

int Telnet_connected(void)
{
    return client_fd > 0 ? 1 : 0;
}

enum
{
    FSM_IDLE,
    FSM_OPT,
    FSM_CMD,
    FSM_SUB_CMD,
    FSM_SUB_CMD_OPT,
} IAC_FSM;

static uint8_t IAC_OPT;

static void proc_cmd(uint8_t op, uint8_t cmd)
{
    (void) op;
    (void) cmd;
}

static uint8_t proc_IAC(uint8_t data)
{
    switch (IAC_FSM)
    {
    case FSM_IDLE:
        if (data == 0xff)
        {
            IAC_FSM = FSM_OPT;
            return 0xff;
        }
        else
        {
            IAC_FSM = FSM_IDLE;
            return data == 0 ? 0xff : data;
        }
    case FSM_OPT:
        switch (data)
        {
        case DONT:
        case DO:
        case WONT:
        case WILL:
            IAC_FSM = FSM_CMD;
            IAC_OPT = data;
            return 0xff;
        case SB:
            IAC_FSM = FSM_SUB_CMD;
            return 0xff;
        default:
            IAC_FSM = FSM_IDLE;
            return 0xff;
        }
    case FSM_SUB_CMD:
        if (data == 0xff)
        {
            IAC_FSM = FSM_SUB_CMD_OPT;
        }
        return 0xff;
    case FSM_SUB_CMD_OPT:
        IAC_FSM = FSM_IDLE;
        return 0xff;
    case FSM_CMD:
        IAC_FSM = FSM_IDLE;
        proc_cmd(IAC_OPT, data);
        return 0xff;
    }
    return 0xff;
}
